package trafficmanager

import (
	"context"
	"math"
	"sort"
	"sync"
	"time"

	"github.com/kyroy/kdtree"
	"github.com/kyroy/kdtree/kdrange"

	"dev.ixmxm.top/rms/boot"
	"dev.ixmxm.top/rms/codes"
	"dev.ixmxm.top/rms/managers/devicemanager"
	"dev.ixmxm.top/rms/managers/robotmanager"
	"dev.ixmxm.top/rms/managers/taskmanager"
	"dev.ixmxm.top/rms/model"
	"dev.ixmxm.top/rms/proto"
	"dev.ixmxm.top/rms/utils/angles"
	dijkstra "dev.ixmxm.top/rms/utils/dijkstra4"
	"dev.ixmxm.top/rms/utils/log"
	"dev.ixmxm.top/rms/utils/tools"
)

func newPlaner(m *model.Map, db *boot.MongoDB, devManager *devicemanager.DeviceManager, taskManager *taskmanager.TaskManager) *Planer {
	//将终点前一个点绑定终点和终点的绑定点
	m.MapData.HandleMapAdjustFrontAGV_1()
	m.MapData.ParseMapNodeAssociatedNodeBlocks()
	planer := &Planer{m: m, graphs: make(map[int]*dijkstra.Graph, 0), db: db, devManager: devManager, taskManager: taskManager}
	planer.buildNodeTree()
	planer.occupiedArea = make(map[string]occupiedAreaObj, 0)
	planer.agvNeedPaths = make(map[string][]string, 0)
	planer.groundLocationMaps = make(map[string]*model.Location, 0)
	// planer.workAreas = make(map[string]model.Area, 0)
	planer.conflictPathIDMap = make(map[string]struct {
		heat              float64
		sn_need           string
		sn_have           string
		startConflictTime time.Time //开始冲突时间
		lastConflictTime  time.Time //最后冲突时间
	}, 0)
	return planer
}

type Planer struct {
	db                       *boot.MongoDB
	devManager               *devicemanager.DeviceManager
	taskManager              *taskmanager.TaskManager
	m                        *model.Map
	groundLocationMaps       map[string]*model.Location
	graphs                   map[int]*dijkstra.Graph
	nodeTree                 *kdtree.KDTree
	localPathLocker          sync.Mutex
	workAreasLocker          sync.Mutex
	initGroundLocationLocker sync.Mutex
	occupiedArea             map[string]occupiedAreaObj
	conflictPathIDMap        map[string]struct {
		heat              float64
		sn_need           string
		sn_have           string    //AGV编号
		startConflictTime time.Time //开始冲突时间
		lastConflictTime  time.Time //最后冲突时间
	}
	conflictPathIDMapLock sync.Mutex
	agvNeedPaths          map[string][]string
	pathLock              sync.Mutex
}

// type locationArea struct {
// 	//将获得的数据初始化赋值

// }
func (planer *Planer) FindGroupLocatinByNodeId(id string) (*model.Location, map[string]model.Location) {
	IsContain := func(list []string, str string) bool {
		for _, strValue := range list {
			if strValue == str {
				return true
			}
		}
		return false
	}
	relationLocation := make(map[string]model.Location, 0)
	for _, groupLocation := range planer.groundLocationMaps {

		arr := make([]string, 0)
		arr = append(arr, groupLocation.DepthPointId...)
		arr = append(arr, groupLocation.FrontPointId)
		arr = append(arr, groupLocation.ContainDotIds...)
		if IsContain(arr, id) {
			var associatePoints []string
			if len(groupLocation.DepthPointId) > 0 {
				depthNode := planer.GetNode(groupLocation.DepthPointId[0])
				associatePoints = depthNode.BanNodes
			} else {

				associatePoints = planer.GetNode(groupLocation.FrontPointId).BanNodes
			}

			if associatePoints != nil && len(associatePoints) > 0 {
				//记录已经计算过的点
				filterDict := make(map[string]string, 0)
				for _, filter := range arr {
					filterDict[filter] = filter
				}
				for _, asId := range associatePoints {
					if _, find := filterDict[asId]; find {
						continue
					}
					findRelationLocation := planer.FindRelationGroupLocatinByNodeId(asId)
					if findRelationLocation != nil {
						relationLocation[findRelationLocation.Group] = *findRelationLocation
						arr1 := make([]string, 0)
						arr1 = append(arr1, groupLocation.DepthPointId...)
						arr1 = append(arr1, groupLocation.FrontPointId)
						arr1 = append(arr1, groupLocation.ContainDotIds...)
						for _, filter1 := range arr1 {
							filterDict[filter1] = filter1
						}
					}
				}

			}

			return groupLocation, relationLocation
		}

	}
	return nil, relationLocation
}

func (planer *Planer) FindRelationGroupLocatinByNodeId(id string) *model.Location {
	IsContain := func(list []string, str string) bool {
		for _, strValue := range list {
			if strValue == str {
				return true
			}
		}
		return false
	}

	for _, groupLocation := range planer.groundLocationMaps {
		arr := make([]string, 0)
		arr = append(arr, groupLocation.DepthPointId...)
		arr = append(arr, groupLocation.FrontPointId)
		arr = append(arr, groupLocation.ContainDotIds...)
		if IsContain(arr, id) {
			return groupLocation
		}
		// if IsContain(groupLocation.ContainDotIds, id) {
		// 	return groupLocation
		// }
	}
	return nil
}

func (planer *Planer) initLocationArea(locations []*model.Location) {
	planer.initGroundLocationLocker.Lock()
	for _, groundLocation1 := range locations {
		//先计算当前所有地堆库位点的
		planer.groundLocationMaps[groundLocation1.Group] = groundLocation1
	}
	planer.initGroundLocationLocker.Unlock()
}

type occupiedAreaObj struct {
	id           string
	mainTaskCode string
}

func (planer *Planer) GetMap() model.Map {
	return *planer.m
}
func (planer *Planer) GetAreaByAreaType(areaType model.AreaType) []model.Area {
	areaArr := make([]model.Area, 0)
	for _, area := range planer.m.MapData.Areas {
		if area.Type == areaType && len(area.ContainDotIds) > 0 {
			areaArr = append(areaArr, area)
		}
	}
	return areaArr
}
func (planer *Planer) GetAreaByAreaTypes(areaTypes ...model.AreaType) []model.Area {
	inArea := func(areaType []model.AreaType, curAreaType model.AreaType) bool {
		for _, areaType := range areaType {
			if areaType == curAreaType {
				return true
			}
		}
		return false
	}
	areaArr := make([]model.Area, 0)
	for _, area := range planer.m.MapData.Areas {
		if inArea(areaTypes, area.Type) && len(area.ContainDotIds) > 0 {
			areaArr = append(areaArr, area)
		}
	}
	return areaArr
}

func (planer *Planer) GetAreaContainIdsByAreaTypes(areaTypes ...model.AreaType) map[string]string {
	inArea := func(areaType []model.AreaType, curAreaType model.AreaType) bool {
		for _, areaType := range areaType {
			if areaType == curAreaType {
				return true
			}
		}
		return false
	}
	contaionIdMap := make(map[string]string, 0)
	for _, area := range planer.m.MapData.Areas {
		if inArea(areaTypes, area.Type) && len(area.ContainDotIds) > 0 {
			for _, id := range area.ContainDotIds {
				contaionIdMap[id] = id
			}
		}
	}
	return contaionIdMap
}
func (planer *Planer) GetCurrentSiteIdArea(currentSiteId string, areaType model.AreaType) []model.Area {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}
	areaArr := make([]model.Area, 0)
	for _, area := range planer.m.MapData.Areas {
		if area.Type == areaType && len(area.ContainDotIds) > 0 && inArea(currentSiteId, area.ContainDotIds) {
			areaArr = append(areaArr, area)
		}
	}
	return areaArr
}
func (planer *Planer) GetCurrentSiteIdContaintFrontArea(currentSiteId string, areaType model.AreaType) []model.Area {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}
	areaArr := make([]model.Area, 0)
	for _, area := range planer.m.MapData.Areas {
		if area.Type != areaType {
			continue
		}
		if len(area.ContainDotIds) > 0 && inArea(currentSiteId, area.ContainDotIds) {
			areaArr = append(areaArr, area)
			continue
		}
		if len(area.FrontLookPoints) > 0 && inArea(currentSiteId, area.FrontLookPoints) {
			areaArr = append(areaArr, area)
		}
	}
	return areaArr
}
func (planer *Planer) GetCurrentSiteIsInArea(currentSiteId string, areaType model.AreaType) bool {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}

	for _, area := range planer.m.MapData.Areas {
		if area.Type == areaType && len(area.ContainDotIds) > 0 && inArea(currentSiteId, area.ContainDotIds) {
			return true
		}
	}
	return false
}

func (planer *Planer) GetAllCanAvoidWaitingOrAvoidNode(robot *model.Robot) []model.NodeD {

	list := make([]model.NodeD, 0)
	for _, value := range planer.m.MapData.NodesMap {
		if value.Type == model.NodeWaiting || value.Type == model.NodeAvoid {
			//允许避让，将点加入
			if value.AllowAvoid == 1 {
				//得到的姿态到，目标位置的带价值
				// if robot.StatusIsExists() {
				// 	return make([]model.NodeD, 0)
				// }
				currentCost, errCurrentGPlan, _ := planer.getBestGlobalPathCost(context.Background(), robot, robot.Status.SiteIdS, value.Id, nil)
				//计算出来带价值保存到集合中
				if errCurrentGPlan != nil {
					continue
				}
				list = append(list, model.NodeD{
					CurrentNode: value,
					Cost:        currentCost,
				})

			}
		}
	}
	if len(list) > 1 {
		//按照带价值升序排序
		sort.Slice(list, func(i, j int) bool {
			return list[i].Cost < list[j].Cost
		})
	}
	return list

}

func (planer *Planer) IsDeadWayAreaCanNotGo(taskDesSite string, currentStandby *model.StandbySite) bool {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}
	//判断当前点是否在死胡同区域内
	deadAreas := planer.GetAreaByAreaType(model.AreaTypeDeadWay)
	if len(deadAreas) == 0 {
		return false
	}
	for _, deadArea := range deadAreas {
		//找出目标点的死胡同
		if inArea(taskDesSite, deadArea.ContainDotIds) && inArea(currentStandby.SiteId, deadArea.ContainDotIds) {
			return true
		}
	}
	return false
}
func (planer *Planer) IsDeadWayAreaAvoidCanNotGo(avoidId string, targetId string) bool {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}
	//判断当前点是否在死胡同区域内
	deadAreas := planer.GetAreaByAreaType(model.AreaTypeDeadWay)
	if len(deadAreas) == 0 {
		return false
	}
	for _, deadArea := range deadAreas {
		//找出目标点的死胡同
		if inArea(avoidId, deadArea.ContainDotIds) && inArea(targetId, deadArea.ContainDotIds) {
			return true
		}
	}
	return false
}

func (planer *Planer) IsDeadWayAreaAvoidCanNotIn(avoidId string, robotSite string) bool {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}
	//判断当前点是否在死胡同区域内
	deadAreas := planer.GetAreaByAreaType(model.AreaTypeDeadWay)
	if len(deadAreas) == 0 {
		return false
	}
	robotInDeadArea, avoidInDeadArea := false, false
	for _, deadArea := range deadAreas {
		//找出目标点的死胡同
		if inArea(avoidId, deadArea.ContainDotIds) {
			avoidInDeadArea = true
			break
		}
	}
	for _, deadArea := range deadAreas {
		if inArea(robotSite, deadArea.ContainDotIds) {
			robotInDeadArea = true
			break
		}
	}
	if !robotInDeadArea && avoidInDeadArea {
		return true
	}
	return false
}
func (planer *Planer) IsInDeadArea(siteId string) (bool, *model.Area) {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}
	//判断当前点是否在死胡同区域内
	deadAreas := planer.GetAreaByAreaType(model.AreaTypeDeadWay)
	if len(deadAreas) == 0 {
		return false, nil
	}
	for _, deadArea := range deadAreas {
		//找出目标点的死胡同
		if inArea(siteId, deadArea.ContainDotIds) {
			return true, &deadArea
		}
	}
	return false, nil
}

func (planer *Planer) IsMutexAreaCanNotGo(taskDesSite string, currentStandby *model.StandbySite) bool {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}
	//判断当前点是否在死胡同区域内
	mutexAreas := planer.GetAreaByAreaType(model.AreaTypeMutex)
	if len(mutexAreas) == 0 {
		return false
	}
	for _, mutexArea := range mutexAreas {
		//找出目标点的死胡同
		if inArea(taskDesSite, mutexArea.ContainDotIds) && inArea(currentStandby.SiteId, mutexArea.ContainDotIds) {
			return true
		}
	}
	return false
}

//查询死胡同下所有待命点集合
func (planer *Planer) GetStandSiteInDeadWayArea() map[string][]string {
	return planer.GetPointMapsInArea(model.NodeWaiting, model.AreaTypeDeadWay)
}

//查询死胡同下所有避让点的集合
func (planer *Planer) GetAavoidSiteInDeadWayArea() map[string][]string {
	return planer.GetPointMapsInArea(model.NodeAvoid, model.AreaTypeDeadWay)
}

//获得当前死胡同区域包含特殊点
func (planer *Planer) GetPointMapsInArea(nodeType model.NodeType, areaType model.AreaType) map[string][]string {
	retmap := make(map[string][]string, 0)
	//获得当前区域类型
	areas := planer.GetAreaByAreaType(areaType)
	if len(areas) == 0 {
		return nil
	}
	//找出映射
	for _, area := range areas {
		contaiontIds := make([]string, 0)
		for _, id := range area.ContainDotIds {
			node := planer.GetNode(id)
			if node.Type == nodeType {
				//设置
				contaiontIds = append(contaiontIds, node.Id)
			}
		}
		retmap[area.Id] = contaiontIds
	}
	return retmap

}

func (planer *Planer) GetNode(id string) model.Node {
	return planer.m.MapData.NodesMap[id]
}
func (planer *Planer) GetNode1(id string) *model.Node {
	findNode, find := planer.m.MapData.NodesMap[id]
	if find {
		return &findNode
	}
	return nil
}

func (planer *Planer) GetTaskTargetNode(id string) *model.Node {
	if value, find := planer.m.MapData.NodesMap[id]; find {
		return &value
	}
	return nil
}

// 构建kdtree用于周围点的获取
func (planer *Planer) buildNodeTree() {
	nodes := make([]kdtree.Point, 0)
	for _, node := range planer.m.MapData.NodesMap {
		treeNode := node
		nodes = append(nodes, &treeNode)
	}
	planer.nodeTree = kdtree.New(nodes)

	//planer.nodeTree.Balance()
}

// 查找坐标在x1<=x=x2  y1<=y <= y2 的点
func (planer *Planer) NodeRangeSearch(x1, x2, y1, y2 float64) (nodes []*model.Node) {
	points := planer.nodeTree.RangeSearch(kdrange.New(x1, x2, y1, y2))
	for _, p := range points {
		nodes = append(nodes, p.(*model.Node))
	}
	//log.Debugf("nodes tree is %s", planer.nodeTree.ToString())
	return nodes
}

// 根据点位Id查找连接的路径
func (planer *Planer) GetLinePathsByDotId(dot *model.Node) map[string]model.Path {
	paths := make(map[string]model.Path, 0)
	for _, p := range dot.ConnectPaths {
		v := planer.m.MapData.Paths[p]
		if v.Type != model.PathTypeLine {
			continue
		}
		paths[v.PathId] = v
	}
	return paths
}

func (planer *Planer) Reach(ctx context.Context, robot *model.Robot, pathID string, reachType proto.IdType, localRejects map[string]string) (*model.GlobalPath, *model.LocalPath, bool, string, codes.Code) {
	if !robot.StatusIsExists() {
		return nil, nil, false, "", codes.ErrDeviceStatusNotExists
	}
	//旋转处理
	switch reachType {
	case proto.IdType_StartRotate:
		//局部路径规划失败,减少锁定点
		currLPath, errGetLPath := planer.getLocalPathFromDB(ctx, robot.Account.AgvId)
		if errGetLPath != nil {
			//log.Panicf("reach get robot:%s local path error:%s", robot.Account.AgvId, errGetLPath.Error())
			log.Errorf("reach get robot:%s local path error:%s", robot.Account.AgvId, errGetLPath.Error())
			return nil, nil, false, "", nil
		}
		if currLPath.Paths.Front() == nil {
			log.Warnf("robot:%s front path is nil", robot.Account.AgvId)
			//return nil, nil, false, codes.ErrPathNotMatch
			return nil, nil, false, "", nil
		}
		if currLPath.Paths.Front().PathId != pathID {
			log.Warnf("robot:%s local plan paths front id is not match : [need:%s get:%s]", robot.Account.AgvId, currLPath.Paths.Front().PathId, pathID)
			//return nil, nil, false, codes.ErrPathNotMatch
			return nil, nil, false, "", nil
		}
		currLPath.Paths.Front().SourceBlocks = nil
		currLPath.CurrentNode = model.Node{}
		currLPath.NodeRect = make(model.PoseArray, 0)
		currLPath.RobotAng = float64(robot.Status.Angle)
		// currLPath.RotateRadius = 0.0
		planer.saveLocalPathToDB(ctx, currLPath, false, false, robot)
		return nil, nil, false, "", nil
	case proto.IdType_EndRotate:
	}

	//获得当前全局路径
	gPaths, errGetG := planer.GetRobotGlobalPaths(ctx, robot.Account.AgvId)
	if errGetG != nil {
		log.ErrorfWithContext(ctx, "get robot:%s global plan error:%s", robot.Account.AgvId, errGetG.Error())
		return nil, nil, false, "", errGetG
	}

	isFinal := gPaths.IsFinalGoal()
	agv_node, _ := planer.m.MapData.NodesMap[robot.Status.SiteIdS]
	current_node := gPaths.Paths.GotPathTarget(pathID, agv_node)

	gPaths.Paths.PopN(pathID) //剔除当前已完成路径
	defer func() {
		if len(gPaths.Paths) > 0 {
			planer.saveGlobalPathToDB(ctx, gPaths)
		} else {
			planer.DeleteRobotGlobalPaths(ctx, robot.Account.AgvId, robot.Status.MapName)
		}
	}()
	robot.AgvCorverNodes = make([]string, 0)
	robot.AgvCorverPaths = make([]string, 0)
	GotRoBotNodeRect := func(r *model.Robot, node model.Node) []model.Pose2 {
		thetaOPRad := angles.Deg2Rad(float64(r.Status.Angle))
		//计算一次最大安全区域
		IsUseMaxHeadRange := false
		x1, x2, y1, y2 := robot.Type.GotRobotPolygonRangeXY(robot, thetaOPRad, float64(node.X), float64(node.Y))
		robotRangeNodes := planer.NodeRangeSearch(x1, x2, y1, y2)
		for _, p1 := range robotRangeNodes {
			if r.Type.IsLockHeadMaxRectangle(*p1, robot, thetaOPRad, float64(node.X), float64(node.Y), 0, node.Type) {
				//判断当前点连接路径
				pathArr := planer.GetLinePathsByDotId(p1)
				if len(pathArr) >= 3 {
					IsUseMaxHeadRange = true
					break
				}
			}
		}
		rect := r.Type.GotLockRectangle(r, thetaOPRad, float64(node.X), float64(node.Y), 0, node.Type, true, IsUseMaxHeadRange)
		for _, point := range robotRangeNodes {
			if r.Type.InLockRectangle(*point, robot, thetaOPRad, float64(node.X), float64(node.Y), 0, node.Type, true) {
				r.AgvCorverNodes = append(r.AgvCorverNodes, point.Id)
			}
		}

		for key, p := range planer.m.MapData.Paths {
			if r.Type.PathConflictRect(r, p, thetaOPRad, float64(node.X), float64(node.Y), true) {
				exist := false
				for _, id := range r.AgvCorverPaths {
					if key == id {
						exist = true
					}
				}
				if !exist {
					r.AgvCorverPaths = append(r.AgvCorverPaths, key)
				}
			}
		}
		return rect
	}
	node_rect := GotRoBotNodeRect(robot, current_node)
	robotr := robot.Type.GetRobotBackRotateRadius(robot)
	if len(gPaths.Paths) == 0 {
		//判断最后一个点,锁定终点锁定点
		// node_rect := GotRoBotNodeRect(robot, current_node)
		lPath := &model.LocalPath{Sn: robot.Account.AgvId, MapName: robot.Status.MapName, CurrentNode: current_node, NodeRect: node_rect, RotateRadius: 0.0, BackRadius: robotr, RobotAng: float64(robot.Status.Angle), SpecialLocks: gPaths.EndBlocks}
		return nil, nil, isFinal, "", planer.saveLocalPathToDB(ctx, lPath, false, false, robot)
	}
	currLPath, errGetLPath := planer.getLocalPathFromDB(ctx, robot.Account.AgvId)
	if errGetLPath != nil {
		log.Panicf("reach get robot:%s local path error:%s", robot.Account.AgvId, errGetLPath.Error())
	}
	lPath, errLocalPlan, blockAgv := planer.localPathPlan(ctx, robot, robot.Status.MapName, gPaths, localRejects, true, true, true)
	if errLocalPlan != nil && errLocalPlan != codes.ErrCallDevice {
		log.Errorf("robot:%s local plan error:%s", robot.Account.AgvId, errLocalPlan.Error())
		return nil, nil, false, "", errLocalPlan
	}
	if len(lPath.Paths) == 0 {
		//局部路径规划失败,减少锁定点
		if currLPath.Paths == nil {
			return gPaths, nil, false, "", nil
		}
		// node_rect := GotRoBotNodeRect(robot, current_node)
		next_path := gPaths.Paths.Front()
		next_path_theta := float64(next_path.RotateStartAng)
		newLocks := planer.reduceLocksRelTarget(robot, ctx, next_path.GetSource(), currLPath.Paths.Back().AllBlocks(), angles.Deg2Rad(float64(robot.Status.Angle)), next_path_theta, robot.Type)
		return gPaths, nil, false, blockAgv, planer.saveLocalPathToDB(ctx, &model.LocalPath{
			Sn:           robot.Account.AgvId,
			MapName:      robot.Status.MapName,
			Paths:        nil,
			CurrentNode:  current_node,
			RotateRadius: 0.0,
			BackRadius:   robotr,
			RobotAng:     float64(robot.Status.Angle),
			NodeRect:     node_rect,
			SpecialLocks: newLocks,
		}, false, false, robot)
	}
	return gPaths, lPath, false, "", nil
}

func (planer *Planer) GetLocalPlanerAreaRejectNodes(ctx context.Context, robot *model.Robot, needTrafficPath *model.TrafficPath, localPaths []model.LocalPath) map[string]string {
	robotId := robot.Account.AgvId
	robotInList := func(agv string, agvList []string) bool {
		for _, id := range agvList {
			if id == agv {
				return true
			}
		}
		return false
	}
	rejectNodes := make(map[string]string, 0)
	addRejects := func(ContainDotIds []string) {
		for _, id := range ContainDotIds {
			rejectNodes[id] = id
		}
	}
	computeNeedPathPositive := func(pathList []model.Path, currentTrafficPath *model.TrafficPath, alowRotate int) int {
		for _, p := range pathList {
			if currentTrafficPath.Id == p.Id {
				if currentTrafficPath.Dir == p.Dir {
					return 1
				} else {
					return 2
				}
			}
		}
		//判断当车身方向
		if alowRotate == 0 {
			if robot.StatusIsExists() {
				for _, p := range pathList {
					if p.GetSource().Id == robot.Status.SiteIdS || p.GetTarget().Id == robot.Status.SiteIdS {
						if math.Abs(angles.NormalizeAngle(p.BodyThetaRad()-angles.Deg2Rad(float64(robot.Status.Angle)))) < angles.Deg2Rad(10.0) {
							return 1
						} else {
							return 2
						}
					}
				}
			}
		}
		//判断需要路线的目标点是否在潮汐起点或者终点
		// log.Warnf("myid:%s 潮汐起点:%s", myid, pathList[0].GetSource().Id)
		// ids := currentTrafficPath.AllBlocks().ToIds()
		// for _, my_id := range ids {
		// 	if pathList[0].GetSource().Id == my_id {
		// 		return 1
		// 	}
		// 	if pathList[len(pathList)-1].GetTarget().Id == my_id {
		// 		return 2
		// 	}

		// }

		//如果路径终点在路径上
		return 3
	}

	planer.workAreasLocker.Lock()
	defer planer.workAreasLocker.Unlock()
	retWorkAreas, _ := planer.PlanerArea(ctx, robotId, localPaths, true)
	//先计算加个互斥锁
	// planer.workAreas
	for _, area := range retWorkAreas {
		if area.Type == model.AreaTypeTide {
			if needTrafficPath != nil {
				//计算当前路径所需潮汐方向
				currentAgvLockMethod := computeNeedPathPositive(area.ContainPathIds, needTrafficPath, area.AlowRotate)
				// log.Warnf("当前潮汐区域朝向：%d ,AGV当前路径计算潮汐朝向：%d", area.LockMethod, currentAgvLockMethod)
				if (area.LockMethod == 1 && currentAgvLockMethod == 2) || (area.LockMethod == 2 && currentAgvLockMethod == 1) {
					log.Warnf("当前潮汐区域朝向：%d ,AGV当前路径计算潮汐朝向：%d", area.LockMethod, currentAgvLockMethod)
					addRejects(area.ContainDotIds)
				}
			}
		}
		if area.Type == model.AreaTypeInteractive && area.AreaWork {

			addRejects(area.ContainDotIds)
			// log.Warnf("robot:%s,受第三方交互区域：%s 影影响，剔除区域内部所有点", robotId, area.Id)
		} else if (area.Type == model.AreaTypeMutex || area.Type == model.AreaTypeTaskLimitRobotArea || area.Type == model.AreaTypeRobotLimitCount) && !robotInList(robotId, area.AgvList) {
			addRejects(area.ContainDotIds)
			// log.Warnf("robot:%s,受互斥区域：%s 影响，剔除区域内部所有点", robotId, area.Id)
		} else if area.Type == model.AreaTypeTide && !robotInList(robotId, area.AgvList) && area.LockMethod == 3 {
			addRejects(area.ContainDotIds)
			// log.Warnf("robot:%s,受潮汐区域：%s 影响，剔除区域内部所有点", robotId, area.Id)
		} else if area.Type == model.AreaTypeBan && robotInList(robotId, area.AgvList) {
			addRejects(area.ContainDotIds)
			// log.Warnf("robot:%s,受禁行区域：%s 影响，剔除区域内部所有点", robotId, area.Id)
		}

	}
	return rejectNodes
}

func (planer *Planer) IsGoToDoorArea(needTrafficPath *model.TrafficPath, targetId string) (bool, *model.Area) {
	allDoorArea := planer.GetAreaByAreaType(model.AreaTypeDoorArea)
	if allDoorArea == nil || len(allDoorArea) < 1 {
		return false, nil
	}
	for _, curArea := range allDoorArea {
		// startFlag := tools.StringSliceIsContain(curArea.ContainDotIds, needTrafficPath.GetSource().Id)
		endFlag := tools.StringSliceIsContain(curArea.ContainDotIds, needTrafficPath.GetTarget().Id)
		targetFlag := tools.StringSliceIsContain(curArea.ContainDotIds, targetId)
		if endFlag && targetFlag {
			return true, &curArea
		}
	}
	return false, nil
}

//判断潮汐
func (planer *Planer) IsTideCanNotGo(currentTrafficPath *model.TrafficPath, needTrafficPathArr model.TrafficPaths, index int, localPaths []model.LocalPath) bool {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId {
				return true
			}
		}
		return false
	}
	computePathPositive := func(pathList []model.Path, nextTrafficPath *model.TrafficPath) int {
		for _, p := range pathList {
			if nextTrafficPath.Id == p.Id {
				if nextTrafficPath.Dir == p.Dir {
					return 1
				} else {
					return 2
				}
			}
		}
		return -1
	}

	planer.workAreasLocker.Lock()
	defer planer.workAreasLocker.Unlock()
	ctx := context.Background()
	retWorkAreas, _ := planer.PlanerArea(ctx, "", localPaths, true)
	isCanNotGo := false
	//先计算加个互斥锁
	for _, area := range retWorkAreas {
		if area.Type == model.AreaTypeTide && currentTrafficPath != nil {
			if !inArea(currentTrafficPath.GetSource().Id, area.ContainDotIds) && inArea(currentTrafficPath.GetTarget().Id, area.ContainDotIds) {
				//判断是否有下一条路径
				if index != len(needTrafficPathArr)-1 {
					nextTrafficPath := needTrafficPathArr[index+1]
					if nextTrafficPath != nil && inArea(nextTrafficPath.GetTarget().Id, area.ContainDotIds) {
						//判断下条路径
						nextLockMethod := computePathPositive(area.ContainPathIds, nextTrafficPath)
						log.Errorf("0000000000000000000000当前潮汐区域朝向：%d ,计算下条路径潮汐区域内的方向：%d", area.LockMethod, nextLockMethod)
						if (area.LockMethod == 1 && nextLockMethod == 2) || (area.LockMethod == 2 && nextLockMethod == 1) {
							return true
						}
					}
				} else {
					return true
				}
			}
		}

	}
	return isCanNotGo
}

func (planer *Planer) GetPlanerAreaRejectNodes(ctx context.Context, robot *model.Robot, needTrafficPath *model.TrafficPath) (map[string]string, codes.Code) {
	inArea := func(currentId string, ContainDotIds []string) bool {
		for _, id := range ContainDotIds {
			if id == currentId {
				return true
			}
		}
		return false
	}
	robotId := robot.Account.AgvId
	robotInList := func(agv string, agvList []string) bool {
		for _, id := range agvList {
			if id == agv {
				return true
			}
		}
		return false
	}
	rejectNodes := make(map[string]string, 0)
	addRejects := func(ContainDotIds []string) {
		for _, id := range ContainDotIds {
			rejectNodes[id] = id
		}
	}
	computeNeedPathPositive := func(pathList []model.Path, currentTrafficPath *model.TrafficPath, alowRotate int) int {
		for _, p := range pathList {
			if currentTrafficPath.Id == p.Id {
				if currentTrafficPath.Dir == p.Dir {
					return 1
				} else {
					return 2
				}
			}
		}
		//判断当车身方向
		if alowRotate == 0 {
			if robot.StatusIsExists() {
				for _, p := range pathList {
					if p.GetSource().Id == robot.Status.SiteIdS || p.GetTarget().Id == robot.Status.SiteIdS {
						if math.Abs(angles.NormalizeAngle(p.BodyThetaRad()-angles.Deg2Rad(float64(robot.Status.Angle)))) < angles.Deg2Rad(10.0) {
							return 1
						} else {
							return 2
						}
					}
				}
			}
		}
		//判断需要路线的目标点是否在潮汐起点或者终点
		// log.Warnf("myid:%s 潮汐起点:%s", myid, pathList[0].GetSource().Id)
		ids := currentTrafficPath.AllBlocks().ToIds()
		for _, my_id := range ids {
			if pathList[0].GetSource().Id == my_id {
				return 1
			}
			if pathList[len(pathList)-1].GetTarget().Id == my_id {
				return 2
			}

		}

		//如果路径终点在路径上
		return 3
	}

	planer.workAreasLocker.Lock()
	defer planer.workAreasLocker.Unlock()
	var localPath []model.LocalPath
	retWorkAreas, err1 := planer.PlanerArea(ctx, robotId, localPath, false)
	if err1 != nil {
		return rejectNodes, err1
	}
	// log.Warnf(" :%d", currentAgvLockMethod)
	//先计算加个互斥锁
	for _, area := range retWorkAreas {
		if area.Type == model.AreaTypeTide {
			if needTrafficPath != nil {
				//计算当前路径所需潮汐方向
				currentAgvLockMethod := computeNeedPathPositive(area.ContainPathIds, needTrafficPath, area.AlowRotate)
				log.Warnf("当前潮汐区域朝向：%d ,AGV当前路径计算潮汐朝向：%d", area.LockMethod, currentAgvLockMethod)
				if (area.LockMethod == 1 && currentAgvLockMethod == 2) || (area.LockMethod == 2 && currentAgvLockMethod == 1) {
					addRejects(area.ContainDotIds)
				} else if !inArea(needTrafficPath.GetSource().Id, area.ContainDotIds) && inArea(needTrafficPath.GetTarget().Id, area.ContainDotIds) {
					addRejects(area.ContainDotIds)
				}

			}
		}
		if area.Type == model.AreaTypeInteractive && area.AreaWork {
			addRejects(area.ContainDotIds)
			// log.Warnf("robot:%s,受第三方交互区域：%s 影响，剔除区域内部所有点", robotId, area.Id)
		} else if (area.Type == model.AreaTypeMutex || area.Type == model.AreaTypeTaskLimitRobotArea || area.Type == model.AreaTypeRobotLimitCount) && !robotInList(robotId, area.AgvList) {
			addRejects(area.ContainDotIds)
			// log.Warnf("robot:%s,触发互斥区域：%s 影响，剔除区域内部所有点", robotId, area.Id)
		} else if area.Type == model.AreaTypeTide && !robotInList(robotId, area.AgvList) && area.LockMethod == 3 {
			addRejects(area.ContainDotIds)
			// log.Warnf("robot:%s,触发潮汐区域：%s 影响，剔除区域内部所有点", robotId, area.Id)
		} else if area.Type == model.AreaTypeBan && robotInList(robotId, area.AgvList) {
			addRejects(area.ContainDotIds)
			// log.Warnf("robot:%s,触发禁行区域：%s 影响，剔除区域内部所有点", robotId, area.Id)
		}

	}
	return rejectNodes, nil
}

// func (planer *Planer) LocalQueryPlanerArea(ctx context.Context, agvId string) {
// 	planer.workAreasLocker.Lock()
// 	planer.PlanerArea(ctx, agvId)
// 	planer.workAreasLocker.Unlock()
// }

//区域要先跟据状态
func (planer *Planer) PlanerArea(ctx context.Context, agvId string, localPaths []model.LocalPath, isExit bool) (map[string]model.Area, codes.Code) {
	robotInArea := func(ids, ContainDotIds []string) bool {
		for _, id := range ContainDotIds {
			for _, myid := range ids {
				if id == myid {
					return true
				}
			}
		}
		return false
	}
	computePositive := func(pathList []model.Path, approvePath model.TrafficPaths, allowRotate int, robot *model.Robot) int {
		for _, ap := range approvePath {
			for _, p := range pathList {
				if ap.Id == p.Id {
					if ap.Dir == p.Dir {
						return 1
					} else {
						return 2
					}
				}
			}
		}
		if allowRotate == 0 {
			if robot.StatusIsExists() {
				for _, p := range pathList {
					if p.GetSource().Id == robot.Status.SiteIdS || p.GetTarget().Id == robot.Status.SiteIdS {
						if math.Abs(angles.NormalizeAngle(p.BodyThetaRad()-angles.Deg2Rad(float64(robot.Status.Angle)))) < angles.Deg2Rad(10.0) {
							return 1
						} else {
							return 2
						}
					}
				}
			}
		}
		//路径与区域没交集，判断路径所有锁定点是否包含潮汐区域起点ID，包含则为正向
		for _, myid := range approvePath.AllBlocks().ToIds() {
			// log.Warnf("myid:%s 潮汐起点:%s", myid, pathList[0].GetSource().Id)
			if pathList[0].GetSource().Id == myid {
				return 1
			}
			if pathList[len(pathList)-1].GetTarget().Id == myid {
				return 2
			}
		}

		return 3
	}
	computeRobotPositive := func(pathList []model.Path, robot *model.Robot) int {
		if robot.StatusIsExists() {
			for _, p := range pathList {
				if p.Type != model.PathTypeLine {
					continue
				}
				if p.GetSource().Id == robot.Status.SiteIdS || p.GetTarget().Id == robot.Status.SiteIdS {
					if math.Abs(angles.NormalizeAngle(p.BodyThetaRad()-angles.Deg2Rad(float64(robot.Status.Angle)))) < angles.Deg2Rad(10.0) {
						return 1
					} else {
						return 2
					}
				}
			}
		}
		return 3
	}
	retWorkAreasMap := make(map[string]model.Area, 0)
	var allApprovedLocalPaths []model.LocalPath
	if isExit {
		allApprovedLocalPaths = localPaths
	} else {
		allApprovedLocalPath1s, err1 := planer.GetAllLocalPathFromDB(ctx)
		if err1 != nil {
			return retWorkAreasMap, nil
		}
		allApprovedLocalPaths = allApprovedLocalPath1s
	}
	setArea := func(bindMainTask *model.MainTask) {

		for _, area := range planer.m.MapData.Areas {
			if len(area.ContainDotIds) == 0 {
				continue
			}
			for _, approved := range allApprovedLocalPaths {
				if approved.MapName == planer.m.MapName {
					ids := approved.Paths.AllBlocks().ToIds()
					ids = append(ids, approved.SpecialLocks.ToIds()...)
					if robotInArea(ids, area.ContainDotIds) {
						area.AgvList = append(area.AgvList, approved.Sn)
						if area.Type == model.AreaTypeTide && len(area.ContainPathIds) > 0 {
							r := robotmanager.GetFromCache(approved.Sn)
							if approved.Paths != nil {
								area.LockMethod = computePositive(area.ContainPathIds, approved.Paths, area.AlowRotate, r)
							} else if area.AlowRotate == 0 {
								robot1 := robotmanager.GetFromCache(approved.Sn)
								area.LockMethod = computeRobotPositive(area.ContainPathIds, robot1)
							}
							if area.LockMethod == 1 || area.LockMethod == 2 {
								break
							}
						}
					}
				}
			}

			switch area.Type {
			case model.AreaTypeMutex:
				if len(area.AgvList) >= 1 {
					area.AreaWork = true
				}
			case model.AreaTypeTaskLimitRobotArea:
				if len(area.AgvList) >= 1 {
					area.AreaWork = true
				}
			case model.AreaTypeRobotLimitCount:
				if len(area.AgvList) >= int(area.RobotLimitCount) {
					area.AreaWork = true
				}
			case model.AreaTypeBan:
				area.AgvList = area.BanAgvIds
				area.AreaWork = true
			case model.AreaTypeTide:
				if len(area.AgvList) >= 1 {
					area.AreaWork = true
				}
			case model.AreaTypeInteractive:
				if findOccupyArea, find := planer.occupiedArea[area.Id]; find {
					if bindMainTask != nil && bindMainTask.MainTaskCode != "" && findOccupyArea.mainTaskCode != "" && bindMainTask.MainTaskCode == findOccupyArea.mainTaskCode {
						area.AreaWork = false
					} else {
						area.AreaWork = true
					}
				}
			}

			if area.Type == model.AreaTypeTide {
				if len(area.ContainPathIds) == 0 {
					delete(retWorkAreasMap, area.Id)
					continue
				}
				// log.Warnf("*************-------------车编号：%s,设置区域对象ID:%s,方向：%d，区域占用：%v", agvId, area.Id, area.LockMethod, area.AreaWork)
				retWorkAreasMap[area.Id] = area
				continue
			}
			if area.AreaWork {
				// if area.Type == model.AreaTypeTide {
				// 	log.Warnf("*************-------------车编号：%s,设置区域对象ID:%s,方向：%d，区域占用：%v", agvId, area.Id, area.LockMethod, area.AreaWork)
				// }
				retWorkAreasMap[area.Id] = area

			}
			// else {
			// 	// if area.Type == model.AreaTypeTide {
			// 	// 	log.Warnf("*************-------------车编号：%s,删除区域对象ID:%s,方向：%d,区域占用：%v", agvId, area.Id, area.LockMethod, area.AreaWork)
			// 	// }
			// 	delete(mapPlaner.workAreas, area.Id)
			// }

		}
	}

	// planer.workAreasLocker.Lock()
	//获得当前AGV绑定的主任务编号
	bindTask, errBindTask := planer.taskManager.GetMainTaskByAgvFromDB(context.Background(), agvId)
	if errBindTask != nil {
		return retWorkAreasMap, errBindTask
	}
	setArea(bindTask)
	return retWorkAreasMap, nil
	// planer.workAreasLocker.Unlock()
}

//锁定小车周围的点
func (planer *Planer) LockNodeAround(ctx context.Context, robot *model.Robot, mapName, nodeID string, angle int, robotType model.RobotType, isOccupyElevetor bool, isDoOnline bool) codes.Code {
	//状态复位
	if robot.StatusIsExists() {
		robot.Status.IsBlock = false
		robot.CanBeCancel = false
	}
	planer.localPathLocker.Lock()
	defer planer.localPathLocker.Unlock()
	sn := robot.Account.AgvId
	log.Infof("锁定信号：robot:%s,点位:%s,角度%d", sn, nodeID, angle)
	node, find := planer.m.MapData.NodesMap[nodeID]
	if !find {
		log.Errorf("lock node:%s around failed, node not found", nodeID)
		return codes.ErrMapNodeNotFound
	}
	robotr := robot.Type.GetRobotBackRotateRadius(robot)
	localPath := &model.LocalPath{Sn: sn, MapName: mapName, CurrentNode: node, RotateRadius: 0.0, BackRadius: robotr, RobotAng: float64(angle)}
	localPath.SpecialLocks.Add(node.Id)
	allApprovedLocalPaths, errGetAll := planer.GetAllLocalPathFromDB(ctx)
	if errGetAll != nil {
		return errGetAll
	}
	//todo
	// robotHead := float64(robotType.RotateRadius) + float64(robotType.SafeDis)
	// robotHead := robot.GetFinalLockRangeParams(node)

	//计算一次最大安全区域
	IsUseMaxHeadRange := false
	//计算头部安全范围内的包含点
	thetaOPRad := angles.Deg2Rad(float64(angle))
	x1, x2, y1, y2 := robot.Type.GotRobotPolygonRangeXY(robot, thetaOPRad, float64(node.X), float64(node.Y))
	robotRangeNodes := planer.NodeRangeSearch(x1, x2, y1, y2)
	for _, p1 := range robotRangeNodes {
		if robotType.IsLockHeadMaxRectangle(*p1, robot, thetaOPRad, float64(node.X), float64(node.Y), 0, node.Type) {
			//判断当前点连接路径
			pathArr := planer.GetLinePathsByDotId(p1)
			if len(pathArr) >= 3 {
				IsUseMaxHeadRange = true
				break
			}
		}
	}

	localPath.NodeRect = robotType.GotLockRectangle(robot, thetaOPRad, float64(node.X), float64(node.Y), 0, node.Type, false, IsUseMaxHeadRange)
	robot.AgvCorverNodes = make([]string, 0)
	robot.AgvCorverPaths = make([]string, 0)
	//更新轮廓
	robotType.RefreshRobotStatus(robot, node.Type, IsUseMaxHeadRange)

	for _, p := range robotRangeNodes {
		if robotType.InLockRectangle(*p, robot, thetaOPRad, float64(node.X), float64(node.Y), 0, node.Type, IsUseMaxHeadRange) {
			localPath.SpecialLocks.Add(p.Id)
			localPath.SpecialLocks.Adds(p.AssociatedBlocks)
			robot.AgvCorverNodes = append(robot.AgvCorverNodes, p.Id)
		}
	}
	for key, p := range planer.m.MapData.Paths {
		if robot.Type.PathConflictRect(robot, p, thetaOPRad, float64(node.X), float64(node.Y), IsUseMaxHeadRange) {
			exist := false
			for _, id := range robot.AgvCorverPaths {
				if key == id {
					exist = true
				}
			}
			if !exist {
				robot.AgvCorverPaths = append(robot.AgvCorverPaths, key)
			}
		}
	}
	log.Infof("锁定信号：robot:%s,车体覆盖点集合:%v", sn, robot.AgvCorverNodes)
	log.Infof("锁定信号：robot:%s,车体覆盖线集合:%v", sn, robot.AgvCorverPaths)
	rejectIds, err2 := planer.GetPlanerAreaRejectNodes(ctx, robot, nil)
	if err2 != nil {
		return err2
	}
	ids := localPath.SpecialLocks.ToIds()
	for _, cover := range ids {
		if _, find := rejectIds[cover]; find {
			log.Infof("碰撞:上线车体%s矩形与区域冲突", localPath.Sn)
			return codes.ErrLockConflict
		}
	}
	for _, robotLocalPath := range allApprovedLocalPaths {
		if robotLocalPath.Sn == sn {
			continue
		}

		if robotLocalPath.CurrentNode.Id != "0" && robotLocalPath.CurrentNode.Id != "" {
			if robotLocalPath.NodeRect.ConflictPolygon(localPath.NodeRect) {
				log.Infof("碰撞:上线车体%s矩形与已有车体%s矩形碰撞", localPath.Sn, robotLocalPath.Sn)
				return codes.ErrLockConflict
			}
		}
		if localPath.SpecialLocks.Conflict(robotLocalPath.SpecialLocks) {
			return codes.ErrLockConflict
		}
		for _, item := range robotLocalPath.Paths {
			if localPath.SpecialLocks.Conflict(item.AllBlocks()) {
				return codes.ErrLockConflict
			}
			if localPath.NodeRect.ConflictPolygon(item.PathPolygon) {
				log.Infof("碰撞:车体%s矩形与路径%s矩形碰撞", localPath.Sn, item.Path.PathId)
				return codes.ErrLockConflict
			}
		}
	}
	// if isGoOnline{
	// 	//判断小车离门的位置
	// 	if robot.StatusIsExists() {
	// 		//判断车当前位置到门的距离
	// 	}
	// }

	// log.Infof("robot:%s lock node:%d around result:%s", sn, nodeID, localPath.SpecialLocks)
	return planer.saveLocalPathToDB(ctx, localPath, isOccupyElevetor, isDoOnline, robot)
}

//死胡同内部点位是否可达
func (planer *Planer) IsDeadWayAreaSiteReachable(ctx context.Context, robot *model.Robot, mapName, siteId string, robots []*model.Robot) (bool, codes.Code) {
	inArea := func(pointId string, allPointIds []string) bool {
		for _, id := range allPointIds {
			if id == pointId && planer.GetNode(id).Type != model.NodeWaiting {
				return true
			}
		}
		return false
	}
	//判断目标点在死胡同=>不可达，当前有车位置在死胡同，
	isIn, DeadArea := planer.IsInDeadArea(siteId)
	if isIn {
		//判断执行任务目标点
		allRunningTask, errCode := planer.taskManager.GetAllStatusTasksFromDB(ctx, model.TaskRunningStatus())
		if errCode != nil {
			log.Panicf("get all running tasks error:%s", errCode.UnWrapError())
			return false, nil
		}

		allTargetSiteArr := make([]string, 0)
		for _, task := range allRunningTask {
			if task.MapName == planer.GetMap().MapName {
				if task.AgvId == robot.Account.AgvId {
					continue
				}
				//同一个地图点的集合
				allTargetSiteArr = append(allTargetSiteArr, task.TargetSiteID)
			}
		}
		//获得其它AGV当前所在的位置
		for _, otherAgv := range robots {
			if otherAgv.Account.AgvId == robot.Account.AgvId {
				continue
			}
			if otherAgv.StatusIsExists() && otherAgv.Status.MapName == planer.GetMap().MapName {
				//记录其它所有车的位置
				allTargetSiteArr = append(allTargetSiteArr, otherAgv.Status.SiteIdS)
			}
		}
		if len(allTargetSiteArr) > 0 {
			//判断当前点是否也在该区域内,且不在待命点
			for _, site := range allTargetSiteArr {
				if inArea(site, DeadArea.ContainDotIds) {
					return false, nil
				}
			}
		}

	}
	return true, nil
}
